/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Behaviors.Schemas.Perception;

import Behaviors.Schemas.*;
import Utils.*;
import Robot.*;
import java.util.Vector;
import java.awt.Color;

/**
 * Acts as a short-memory map for the obstacles.
 * Useful in sensor belts where the angular resolution is not wide, as in IR sensors.
 * The memory range is defined all around the robot. Its size is defined by the maxRange
 * variable. Obstacles below the range will be saved, the others not. Each
 * detected obstacle is saved in memory with a probability of existence. Each
 * time an obstacle is detected in the same position, its probability increases.
 * Each obstacle-free reading from the sensors in the directions where obstacles should
 * we detected (obstacles that are in memory), will decrease the probability of the
 * obstacles located in that directions. All obstacles have a defined size. That permits
 * to map fewer objects, because every new object is tested to not be in the area
 * of an already defined object.
 * In each direction there can be only a detected obstacle.
 *
 *
 * @author Alejandro Pustowka
 */
public class PSMapObstacles extends PSDetectObstacles{

    Schema detect;
    Position2d position;
    Vector obstacles;
    
    VectorFrame frame;
    boolean showFrame;

    int maxProb = 100;


    /**
     * Creates a detection behavior for mapping obstacles in memory.
     * @param name Name of the behavior.
     * @param detect Detection behavior from which the obstacles will be readed.
     * @param position Position interface of the robot.
     * @param showFrame Defines if a frame showing detected obstacles should be
     * printed in screen.
     */
    public PSMapObstacles(String name,Schema detect, Position2d position, boolean showFrame)
    {
        super(name);
        this.detect = detect;
        this.position = position;
        this.showFrame = showFrame;
        obstacles = new Vector();
        if(showFrame)
            configFrame();
        
    }

    /**
     * Creates a detection behavior for mapping obstacles in memory.
     * @param name Name of the behavior.
     * @param detect Detection behavior from which the obstacles will be readed.
     * @param position Position interface of the robot.
     * @param memRange size of the memory range. Its value is saved in the maxRange variable.
     * @param obstSize Size of the obstacles.
     * @param showFrame Defines if a frame showing detected obstacles should be
     * printed in screen.
     */
    public PSMapObstacles(String name,Schema detect, Position2d position, float memRange, float obstSize, boolean showFrame)
    {
        super(name);
        this.detect = detect;
        this.position = position;
        this.showFrame = showFrame;
        obstacles = new Vector();

        if(memRange != -1)
            this.maxRange = memRange;
        if(obstSize != -1)
            this.obstSize = obstSize;
        
        if(showFrame)
            configFrame();
    }

    private void configFrame()
    {
        frame = new VectorFrame("Obstaculos",VectorFrame.POLAR_COORD,10,18);

        //draw circle that represents the robot
        frame.addStaticElement("robot", frame.CIRCLE_ELEMENT, Robot.radius, 0.0f, 0.0f, Color.RED);
        //draw circle that represents memory limit
        frame.addStaticElement("memoryLimit",frame.CIRCLE_ELEMENT, maxRange, 0, 0,Color.ORANGE);
        //dont draw table
        frame.drawTable(false);
        frame.setVisible(true);
    }

    /**
     * Defines the maximum probability the objects will handle.
     *
     * @param maxProb The new value for the maxProb variable.
     */
    public void setMaxProb(int maxProb)
    {
        this.maxProb = maxProb;
    }


    

    public Vector2d[] Value()
    {
        Vector2d[] newObst = ((PSDetectObstacles)detect).Value();


        Vector2d robotPos = position.getPosition();
        int length = newObst.length;

        int numObst = obstacles.size();
        
        //first assign directly all obstacles detected by sensors to the array
        for(int i=0;i<length;i++)
        {
            //only take into account the vector elements where there are obstacles.
            if(newObst[i].getMag()>0)
            {
                //review each obstacles in the array in order to avoid redundant obstacles
                if(obstacles.size()!=0)
                {
                     boolean isNew = true;
                     numObst = obstacles.size();
                    for(int j=0;j<numObst;j++)
                    {
                        
                        //get obstacles that are already in the array
                        //Vector2d tempObst = new Vector2d((Vector2d)obstacles.get(j));
                        Obstacle obst = (Obstacle)obstacles.get(j);
                        Vector2d tempObst = new Vector2d(obst.getVector());

                        //calculate difference between the new obstacle
                        Vector2d diffObst = newObst[i].sub(tempObst);
                        
                        if(Math.abs(diffObst.getMag())<this.obstSize)
                        {
                            isNew = false;
                            ((Obstacle)obstacles.get(j)).incProb();
                            
                            break;

                        }
                            

                    }
                    if(isNew)
                    {
                        //review obstacles that are in the direction of this new obstacle
                        //if

                        //copy of obstacle vector, to calculate angle
                        Vector2d nObst = new Vector2d(newObst[i]);

                        nObst = nObst.sub(robotPos);

                        float obstAngle = (float)nObst.getAngle();

                        numObst =obstacles.size();
                        for(int j=0;j<numObst;j++)
                        {

                            //get obstacles that are already in the array
                            //Vector2d tempObst = new Vector2d((Vector2d)obstacles.get(j));
                            Obstacle obst = (Obstacle)obstacles.get(j);
                            Vector2d tempObst = new Vector2d(obst.getVector());

                            //the obstacle is referenced to the robots plane
                            tempObst = tempObst.sub(robotPos);

                            
                            //compare the direction to the obstacle and the direction
                            //of the sensor
                            float diffAngle = (float)tempObst.getAngle() - (float)obstAngle;
                            
                            //if the two direction are close enough, then the probability
                            //of existence of this object must decrease.
                            if(Math.abs(diffAngle)<Math.PI/18)
                            {

                                Obstacle o = (Obstacle)obstacles.get(j);
                                //o.decProb();

                                //if(o.getProb() == 0)
                                //{
                                    obstacles.remove(j);
                                    numObst--;
                                    j--;
                                //}

                                break;

                            }
                        }

                        //obstacles.add(new Vector2d(newObst[i]));
                        obstacles.add(new Obstacle((float)newObst[i].getMag(),(float)newObst[i].getAngle(),1,maxProb,Vector2d.MODE_MA));

                        
                    }
                }
                //First obstacle added to the array
                else
                    //obstacles.add(new Vector2d(newObst[i]));
                    obstacles.add(new Obstacle((float)newObst[i].getMag(),(float)newObst[i].getAngle(),1,maxProb,Vector2d.MODE_MA));

            }
            //now deal with undetected zones. if there are mapped obstacles,
            //decrease its probability of existence.
            else
            {
                //verify if at the position of the sensor are mapped objects

                //first calculate angle of the sensor, related to the reference.
                float freeAngle = (float)newObst[i].getAngle();
                freeAngle = (float)Vector2d.angleCorrMPI2PI(freeAngle);

                if(obstacles.size()!=0)
                {
                    numObst = obstacles.size();
                    for(int j=0;j<numObst;j++)
                    {
                        //get obstacles that are already in the array
                        //Vector2d tempObst = new Vector2d((Vector2d)obstacles.get(j));
                        Obstacle obst = (Obstacle)obstacles.get(j);
                        Vector2d tempObst = new Vector2d(obst.getVector());

                        //the obstacle is referenced to the robots plane
                        tempObst = tempObst.sub(robotPos);

                        //compare the direction to the obstacle and the direction
                        //of the sensor
                        float diffAngle = (float)tempObst.getAngle() - freeAngle;

                        //if the two direction are close enough, then the probability
                        //of existence of this object must decrease.
                        if(Math.abs(diffAngle)<Math.PI/18)
                        {

                            Obstacle o = (Obstacle)obstacles.get(j);
                            o.decProb();
                            
                            if(o.getProb() == 0)
                            {
                                obstacles.remove(j);
                                numObst--;
                                j--;
                            }

                            break;

                        }
                    }
                }
            }
        }

        //now review all obstacles, to see which ones are in the area of influence.
        numObst = obstacles.size();
        for(int i=0;i<numObst;i++)
        {
            //Vector2d tempObst = (Vector2d)obstacles.get(i);
            Vector2d tempObst = (Vector2d)((Obstacle)obstacles.get(i)).getVector();
            Vector2d distToObst = tempObst.sub(robotPos);
            if(Math.abs(distToObst.getMag())>maxRange)
            {
                obstacles.remove(i);
                numObst--;
                i--;
            }

            if(obstacles.size()<1)
                break;

        }

        //Vector2d[] result = (Vector2d[])obstacles.toArray(new Vector2d[obstacles.size()]);

        Obstacle[] res = (Obstacle[])obstacles.toArray(new Obstacle[obstacles.size()]);
        Vector2d[] result = new Vector2d[obstacles.size()];

        for(int i=0;i<obstacles.size();i++)
        {
            result[i] = res[i].getVector();
        }

        if(showFrame)
        {
            Vector2d[] graphVects = new Vector2d[result.length];

                frame.resetDynElements();

            for(int i=0;i<result.length;i++)
            {
                graphVects[i] = result[i].sub(robotPos);
                frame.addDynElement("", frame.CIRCLE_ELEMENT, obstSize, (float)graphVects[i].getX(), (float)graphVects[i].getY(), Color.RED);
            }
        }

        //Calculate orientation of robot
        Pose2d pos = position.read(Position2d.READ_MODE_NO_WAIT);

        //Direction vector of the robot
        if(showFrame)
        {
            frame.addDynElement("",frame.VECTOR_ELEMENT,0, (float)Math.cos(pos.getPa()), (float)Math.sin(pos.getPa()),Color.BLUE);
            frame.repaint();
        }

        if(result == null)
            return new Vector2d[0];
        else
            return result;
        




    }

}


class Obstacle
{
    private Vector2d vector;
    private int prob;
    private int maxProb;

    public Obstacle(float x, float y, int prob, int maxProb, int mode)
    {
        this.prob = prob;
        this.maxProb = Math.abs(maxProb);

        if(maxProb < 0)
            this.prob = 10;

        if(mode == Vector2d.MODE_XY)
        {
            vector = new Vector2d(x,y, Vector2d.MODE_XY);
        }
        else
        {
            vector = new Vector2d(x,y, Vector2d.MODE_MA);
        }

    }

    public void incProb()
    {
        if(prob<maxProb)
            prob++;
    }

    public void decProb()
    {
        if(prob>0 && maxProb>0)
            prob--;
    }

    public int getProb()
    {
        return this.prob;
    }
    public void setVector(Vector2d vec)
    {
        this.vector = vec;
    }

    public Vector2d getVector()
    {
        return this.vector;
    }
}
